#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "mex.h"

/******************************************************************************/
double r8_uniform_01 ( int *seed )
{
    int i4_huge = 2147483647;
    int k;
    double r;
    
    if ( *seed == 0 )
    {
        printf ( "\n" );
        printf ( "R8_UNIFORM_01 - Fatal error!\n" );
        printf ( "  Input value of SEED = 0.\n" );
        exit ( 1 );
    }
    
    k = *seed / 127773;
    
    *seed = 16807 * ( *seed - k * 127773 ) - k * 2836;
    
    if ( *seed < 0 )
    {
        *seed = *seed + i4_huge;
    }
    
    r = ( double ) ( *seed ) * 4.656612875E-10;
    
    return r;
}

/******************************************************************************/
double r8_uniform ( double b, double c, int *seed )
{
    double value;
    
    if ( *seed == 0 )
    {
        printf ( "\n" );
        printf ( "R8_UNIFORM - Fatal error!\n" );
        printf ( "  Input value of SEED = 0.\n" );
        exit ( 1 );
    }
    
    value = b + ( c - b ) * r8_uniform_01 ( seed );
    
    return value;
}

/******************************************************************************/
double r8_normal_01 ( int *seed )
{
    # define PI 3.141592653589793
    
    double r1;
    double r2;
    static int seed1 = 0;
    static int seed2 = 0;
    static int seed3 = 0;
    static int used = 0;
    double v1;
    static double v2 = 0.0;
    
    if ( ( used % 2 ) == 1 )
    {
        if ( *seed != seed2 )
        {
            used = 0;
            seed1 = 0;
            seed2 = 0;
            seed3 = 0;
            v2 = 0.0;
        }
    }
    
    if ( ( used % 2 )== 0 )
    {
        seed1 = *seed;
        
        r1 = r8_uniform_01 ( seed );
        
        if ( r1 == 0.0 )
        {
            printf ( "\n" );
            printf ( "R8_NORMAL_01 - Fatal error!\n" );
            printf ( "  R8_UNIFORM_01 returned a value of 0.\n" );
            exit ( 1 );
        }
        
        seed2 = *seed;
        
        r2 = r8_uniform_01 ( &seed2 );
        
        seed3 = *seed;
        
        v1 = sqrt ( -2.0 * log ( r1 ) ) * cos ( 2.0 * PI * r2 );
        v2 = sqrt ( -2.0 * log ( r1 ) ) * sin ( 2.0 * PI * r2 );
        
        *seed = seed2;
    }
    
    else
    {
        v1 = v2;
        *seed = seed3;
    }
    
    used = used + 1;
    
    return v1;
    # undef PI
}

/******************************************************************************/
double r8_normal ( double a, double b, int *seed )
/*
    The normal probability distribution function (PDF) is sampled,
    with mean A and standard deviation B.
    Input, double A, the mean of the PDF.
    Input, double B, the standard deviation of the PDF.
    Input/output, int *SEED, a seed for the random number generator.
    Output, double R8_NORMAL, a sample of the normal PDF.
 */
{
    double value;
    
    value = a + b * r8_normal_01 ( seed );
    return value;
}

/******************************************************************************/
int convertRC(int row, int col, double mapDimension){
    int index;
    index = (row-1)*mapDimension + col - 1;
    return(index);
}

/******************************************************************************/
void diamondStep(double mapDimension, double maxIndex, double variance, double *TR, int *pSeed){
    
    double sh = mapDimension/2;
    int row,col,ind;
    double value,displacement;
    row=sh+1;
    col=sh+1;
    
    while (row < maxIndex){
        while(col < maxIndex){
            
            value = TR[convertRC(row-sh,col-sh,maxIndex)] + \
            TR[convertRC(row-sh,col+sh,maxIndex)] + \
            TR[convertRC(row+sh,col-sh,maxIndex)] + \
            TR[convertRC(row+sh,col+sh,maxIndex)];
            value = value / 4;
            
            displacement = r8_normal(0,sqrt(variance),pSeed);
            value += displacement;
            
            if (!finite(TR[convertRC(row,col,maxIndex)]))
                TR[convertRC(row,col,maxIndex)] = value;
            
            // next square in same row
            col += mapDimension;
        }
        
        // next row
        col = sh+1;
        row += mapDimension;
    }
}

/******************************************************************************/
void squareStep(double mapDimension, double maxIndex, double variance, double *TR, int *pSeed)
{
    double sh = mapDimension/2;
    int row,col,nop;
    double value,displacement;
    
    int colStart = sh+1;
    row=1;
    col=colStart;           // row, col are the indices
    // of each diamond's centerpoint
    
    while (row <= maxIndex)
    {
        while(col <= maxIndex)
        {
            
            value = 0;
            nop = 4;                // number of points
            
            // the following cases handle the boundary points,
            // i.e. the incomplete diamonds
            
            // north
            if (row > 1)
                value += TR[convertRC(row-sh,col,maxIndex)];
            else
                nop--;
            
            // east
            if (col < maxIndex)
                value += TR[convertRC(row,col+sh,maxIndex)];
            else
                nop--;
            
            // south
            if (row < maxIndex)
                value += TR[convertRC(row+sh,col,maxIndex)];
            else
                nop--;
            
            // west
            if (col > 1)
                value += TR[convertRC(row,col-sh,maxIndex)];
            else
                nop--;
            
            
            // displacement
            displacement = r8_normal(0,sqrt(variance),pSeed);
            value = value/nop + displacement; // why is value divided by nop?
            
            // set square point (if not predefined)
            if (!finite(TR[convertRC(row,col,maxIndex)]))
                TR[convertRC(row,col,maxIndex)] = value;
            
            // next diamond in same row
            col += sh;
        }
        
        // next row
        // the starting column alternates between 1 and sh
        
        if (colStart == 1)
            colStart = sh+1;
        else
            colStart = 1;
        
        col = colStart;
        row = row + sh;
    }
}

void mainSub(double mapDimension, double variance, double h, double *pMap, int *pSeed)
{
    double maxIndex = mapDimension;
    
    mapDimension--;
    while (mapDimension>1)
    {
        diamondStep(mapDimension, maxIndex, variance, pMap, pSeed);
        squareStep(mapDimension, maxIndex, variance, pMap, pSeed);
        
        mapDimension = mapDimension/2;
        variance = variance / pow(2,h);
    }
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )

{
    double initVar,h,mapDimension;
    double *pMap;
    int seed = rand();
    
    mapDimension = *mxGetPr(prhs[0]);
    initVar = *mxGetPr(prhs[1]);
    h = *mxGetPr(prhs[2]);
    pMap = mxGetPr(prhs[3]);
    
    mainSub(mapDimension,initVar,h,pMap,&seed);
    
    return;
    
}
